function [out, auxOut] = kalmfiltstep(mode, in, cfg_fcnMTimes_extrap, cfg_fcnMTimes_upd, cfg, par, iniCon) %#codegen
%KALMFILTSTEP Kalman滤波算法解算周期
%
% Tips:
% # 输入输出参数域在各种运行模式下的使用情况表：
% 参数              初始化引用的域                   外推引用的域              更新引用的域                   状态向量重设引用的域
% in                F、GPQGPT                        F、GPQGPT                 zObs、H、GammaMRGammaMT、      u
%                                                                             enableMeasResidChk、
%                                                                             chiSqChkThreshold
% cfg、par、iniCon  所有                             无                        无                             无
% out               所有                             extrapCycCnt、x、P        updCnt、x、P                   x
% auxOut            无                               Phi，Q                    K                              无
% # 用kalmfiltstep.prj文件build该函数时，需要将输入中cfg_fcnMTimes_extrap、cfg_fcnMTimes_upd两项删除，并在代码中设置
%
% Input Arguments:
% # mode: int8标量，运行模式：0为初始化，1为状态估计及误差协方差外推，2为状态估计及误差协方差更新，3为状态向量重设，其它值输出上次运行结果（此时使用in.F、in.zObs参数用于确定永久变量尺寸）
% # in.F: m*m矩阵，m与iniCon.x的长度相同，动态矩阵
% # in.GPQGPT: m*m矩阵，等价于GP*Q*GPT,其中GP为过程噪声动态耦合矩阵，Q为过程噪声的功率谱密度矩阵，GPT为GP的转置
% # in.zObs: n*1列向量，观测量
% # in.H: n*m矩阵，量测矩阵
% # in.GammaMRGammaMT: n*n矩阵，等价于GammaM*R*GammaMT，其中GammaM为量测噪声耦合矩阵，R为量测噪声序列协方差矩阵，GammaMT为GammaM的转置
% # in.enableMeasResidChk: 逻辑标量，true表示进行量测残差检验，false表示不进行量测残差检验，若量测残差检验不通过，该观测量将不参与更新
% # in.chiSqChkThreshold: 标量，χ^2检验阈值，负值表示不进行χ^2检验，若χ^2检验不通过，该观测量将不参与更新
% # in.u: m*1列向量，控制向量，在状态向量重设模式下通常为负的状态向量
% # in.P0: m*m矩阵，控制向量，P阵重置使用
% # cfg.nPhilambdalambda: 标量，当采用迭代型误差协方差外推计算式时代表Philambdalambda矩阵展开的阶数，参见kalmfiltextrap_iter函数输入参数nPhilambdalambda的帮助；当采用普通的误差协方差外推计算式时代表状态转移矩阵阶数，2为2阶，大于等于3为3阶，其它值为1阶
% # cfg.nPhilambday: 标量，Philambday矩阵展开的阶数，参见kalmfiltextrap_iter函数输入参数nPhilambday的帮助
% # cfg.extrapAlgo: int8标量，外推算法，int8(KalmFiltExtrapAlgo.ITER)采用迭代型误差协方差外推计算式，其它值采用普通的误差协方差外推计算式
% # cfg.updAlgo: int8标量，更新算法，int8(KalmFiltUpdAlgo.JOSEPH)采用约瑟夫型误差协方差更新计算式，其它值采用普通的误差协方差更新计算式
% # cfg.enableNumCondCtrl: 逻辑标量，true表示在计算状态误差协方差矩阵P时进行数值条件控制，使P阵保持对称性及非负定性，防止滤波发散，false表示不进行数值条件控制
% # par.dTExtrap: 标量，卡尔曼滤波外推解算周期，单位s
% # par.PDiagMin: m*1列向量，各元素为状态误差协方差矩阵P对角线元素对应的合理最小值
% # par.PDiagMax: m*1列向量，各元素为状态误差协方差矩阵P对角线元素对应的合理最大值
% # iniCon.x: m*1列向量，状态向量初始值
% # iniCon.P: m*m矩阵，状态误差协方差矩阵初始值
% # cfg_fcnMTimes_extrap: 函数句柄，外推算法中的矩阵乘法函数
% # cfg_fcnMTimes_upd: 函数句柄，更新算法中的矩阵乘法函数
%
% Output Arguments:
% # out.extrapCycCnt: 标量，外推解算周期计数
% # out.updCnt: 标量，更新解算周期计数
% # out.x: m*1列向量，卡尔曼滤波器状态向量
% # out.P: m*m矩阵，卡尔曼滤波器状态误差协方差矩阵
% # out.measResidChkPass: n*1逻辑列向量，true表示对应元素通过量测残差检验，false表示对应元素没有通过量测残差检验
% # out.chiSqChkPass: 逻辑标量，true表示量测通过χ^2检验，false表示没有通过
% # auxOut.measResid: n*1列向量，量测残差，不进行检验时仍输出
% # auxOut.measResidCovMatDiagSqrt: n*1列向量，量测残差协方差矩阵对角线值的平方根，不进行检验时仍输出
% # auxOut.chiSqIndex: 标量，χ^2检验指标值，不进行检验时仍输出
% # auxOut.K: m*n矩阵，卡尔曼滤波器增益矩阵
% # auxOut.Phi: m*m矩阵，卡尔曼滤波器状态转移矩阵
% # auxOut.GammaPQGammaPT: m*m矩阵，等价于GammaP*Q*GammaPT，其中GammaP为过程噪声耦合矩阵，Q为过程噪声序列协方差矩阵，GammaPT为GammaP的转置
%
% References:
% # 《应用导航算法工程基础》“离散型卡尔曼滤波”

persistent p_KFStep_cfg p_KFStep_par p_KFStep_mid p_KFStep_out p_KFStep_auxOut p_KFStep_fcnMTimes_extrap p_KFStep_fcnMTimes_upd

coder.extrinsic('warning','int2str');

% NOTE: 用kalmfiltstep.prj生成代码时使用，一般应屏蔽以下代码
% cfg_fcnMTimes_extrap = @kfextrapdefaultmtimes;
% cfg_fcnMTimes_upd = @kfupddefaultmtimes;

%% persistent变量初始化
% NOTE: 不应依靠在该cell中对persistent变量赋的初值，因该cell仅在persistent变量不存在时执行
if isempty(p_KFStep_cfg) || isempty(p_KFStep_par) || isempty(p_KFStep_mid) || isempty(p_KFStep_out) ...
        || isempty(p_KFStep_auxOut) || isempty(p_KFStep_fcnMTimes_extrap) || isempty(p_KFStep_fcnMTimes_upd)
    p_KFStep_cfg.nPhilambdalambda = coder.nullcopy(NaN);
    p_KFStep_cfg.nPhilambday = coder.nullcopy(NaN);
    p_KFStep_cfg.extrapAlgo = coder.nullcopy(int8(KalmFiltExtrapAlgo.ITER));
    p_KFStep_cfg.updAlgo = coder.nullcopy(int8(KalmFiltUpdAlgo.JOSEPH));
    p_KFStep_cfg.enableNumCondCtrl = coder.nullcopy(true);
    
    p_KFStep_par.dTExtrap = coder.nullcopy(NaN);
    p_KFStep_par.PDiagMin = coder.nullcopy(NaN(size(in.F, 1), 1));
    p_KFStep_par.PDiagMax = coder.nullcopy(NaN(size(in.F, 1), 1));
    
    p_KFStep_mid.extrapCycCnt = coder.nullcopy(NaN);
    p_KFStep_mid.updCnt = coder.nullcopy(NaN);
    p_KFStep_mid.x = coder.nullcopy(NaN(size(in.F, 1), 1));
    p_KFStep_mid.P = coder.nullcopy(NaN(size(in.F)));
    p_KFStep_mid.F_1 = coder.nullcopy(NaN(size(in.F)));
    p_KFStep_mid.GPQGPT_1 = coder.nullcopy(NaN(size(in.F)));
    
    p_KFStep_out.extrapCycCnt = coder.nullcopy(NaN);
    p_KFStep_out.updCnt = coder.nullcopy(NaN);
    p_KFStep_out.x = coder.nullcopy(NaN(size(in.F, 1), 1));
    p_KFStep_out.P = coder.nullcopy(NaN(size(in.F)));
    p_KFStep_out.measResidChkPass = coder.nullcopy(true(size(in.zObs)));
    p_KFStep_out.chiSqChkPass = coder.nullcopy(true);
    
    p_KFStep_auxOut.measResid = coder.nullcopy(NaN(size(in.zObs)));
    p_KFStep_auxOut.measResidCovMatDiagSqrt = coder.nullcopy(NaN(size(in.zObs)));
    p_KFStep_auxOut.chiSqIndex = coder.nullcopy(NaN);
    p_KFStep_auxOut.K = coder.nullcopy(NaN(size(in.F, 1), size(in.zObs, 1)));
    p_KFStep_auxOut.Phi = coder.nullcopy(NaN(size(in.F)));
    p_KFStep_auxOut.GammaPQGammaPT = coder.nullcopy(NaN(size(in.F)));
    
    p_KFStep_fcnMTimes_extrap = cfg_fcnMTimes_extrap;
    p_KFStep_fcnMTimes_upd = cfg_fcnMTimes_upd;
end

switch mode
    case int8(0)
        %% 初始化
        p_KFStep_fcnMTimes_extrap = cfg_fcnMTimes_extrap;
        p_KFStep_fcnMTimes_upd = cfg_fcnMTimes_upd;
        % 配置
        p_KFStep_cfg.nPhilambdalambda = cfg.nPhilambdalambda;
        p_KFStep_cfg.nPhilambday = cfg.nPhilambday;
        p_KFStep_cfg.extrapAlgo = cfg.extrapAlgo;
        p_KFStep_cfg.updAlgo = cfg.updAlgo;
        p_KFStep_cfg.enableNumCondCtrl = cfg.enableNumCondCtrl;
        
        % 参量
        p_KFStep_par.dTExtrap = par.dTExtrap;
        p_KFStep_par.PDiagMin = par.PDiagMin;
        p_KFStep_par.PDiagMax = par.PDiagMax;
        
        % 零时刻初始化
        p_KFStep_mid.extrapCycCnt = 0;
        p_KFStep_mid.updCnt = 0;
        p_KFStep_mid.x = iniCon.x;
        p_KFStep_mid.P = iniCon.P;
        p_KFStep_mid.F_1 = in.F;
        p_KFStep_mid.GPQGPT_1 = in.GPQGPT;
        
        if (size(p_KFStep_mid.P, 1)~=length(p_KFStep_mid.x)) || (size(p_KFStep_mid.P, 2)~=length(p_KFStep_mid.x))
            warning('kalmfiltstep:stateVecLen', 'The length of initial state vector is not the same with the size of error covariance matrix.');
        end
        % 输出量
        p_KFStep_out.extrapCycCnt = p_KFStep_mid.extrapCycCnt;
        p_KFStep_out.updCnt = p_KFStep_mid.updCnt;
        p_KFStep_out.x = p_KFStep_mid.x;
        p_KFStep_out.P = p_KFStep_mid.P;
        p_KFStep_out.measResidChkPass = true(size(in.zObs));
        p_KFStep_out.chiSqChkPass = true;
        % 辅助输出量
        p_KFStep_auxOut.measResid = NaN(size(in.zObs));
        p_KFStep_auxOut.measResidCovMatDiagSqrt = NaN(size(in.zObs));
        p_KFStep_auxOut.chiSqIndex = NaN;
        p_KFStep_auxOut.K = NaN(size(iniCon.x, 1), size(in.zObs, 1));
        p_KFStep_auxOut.Phi = NaN(size(iniCon.P));
        p_KFStep_auxOut.GammaPQGammaPT = NaN(size(iniCon.P));
    case int8(1)
        %% 状态估计及误差协方差外推解算周期
        switch p_KFStep_cfg.extrapAlgo
            case int8(KalmFiltExtrapAlgo.ITER)
                [p_KFStep_mid.x, p_KFStep_mid.P, mid_Phi, p_KFStep_auxOut.GammaPQGammaPT] = kalmfiltextrap_iter(p_KFStep_mid.x, p_KFStep_mid.P, ...
                    (in.F+p_KFStep_mid.F_1)/2*p_KFStep_par.dTExtrap, (in.GPQGPT+p_KFStep_mid.GPQGPT_1)/2*p_KFStep_par.dTExtrap, ...
                    p_KFStep_cfg.nPhilambdalambda, p_KFStep_cfg.nPhilambday);
            otherwise
                [p_KFStep_mid.x, p_KFStep_mid.P, mid_Phi, p_KFStep_auxOut.GammaPQGammaPT] = kalmfiltextrap(p_KFStep_mid.x, p_KFStep_mid.P, ...
                    (in.F+p_KFStep_mid.F_1)/2, (in.GPQGPT+p_KFStep_mid.GPQGPT_1)/2, p_KFStep_par.dTExtrap, p_KFStep_cfg.nPhilambdalambda, p_KFStep_fcnMTimes_extrap);
        end
        p_KFStep_mid.extrapCycCnt = p_KFStep_mid.extrapCycCnt + 1;
        p_KFStep_mid.F_1 = in.F;
        p_KFStep_mid.GPQGPT_1 = in.GPQGPT;
        % 输出
        p_KFStep_out.extrapCycCnt = p_KFStep_mid.extrapCycCnt;
        p_KFStep_out.x = p_KFStep_mid.x;
        p_KFStep_out.P = p_KFStep_mid.P;
        p_KFStep_auxOut.Phi = mid_Phi;
    case int8(2)
        %% 状态估计及误差协方差更新解算周期
        % 量测残差检验与χ^2检验
        [p_KFStep_auxOut.measResid, p_KFStep_auxOut.chiSqIndex, ...
            p_KFStep_out.measResidChkPass, p_KFStep_out.chiSqChkPass, p_KFStep_auxOut.measResidCovMatDiagSqrt] ...
            = chkmeaserr(p_KFStep_mid.x, p_KFStep_mid.P, in.zObs, in.H, in.GammaMRGammaMT, ...
            in.enableMeasResidChk, in.chiSqChkThreshold);
        if p_KFStep_out.chiSqChkPass && all(p_KFStep_out.measResidChkPass)
            % 检验通过进行更新
            switch p_KFStep_cfg.updAlgo
                case int8(KalmFiltUpdAlgo.JOSEPH)
                    [p_KFStep_mid.x, p_KFStep_mid.P, tmp_K] = kalmfiltupd_joseph(p_KFStep_mid.x, p_KFStep_mid.P, in.zObs, in.H, in.GammaMRGammaMT, p_KFStep_fcnMTimes_upd);
                case int8(KalmFiltUpdAlgo.JOSEPH_SIMPLE)
                    [p_KFStep_mid.x, p_KFStep_mid.P, tmp_K] = kalmfiltupd_joseph_simple(p_KFStep_mid.P, in.zObs, in.H, in.GammaMRGammaMT, p_KFStep_fcnMTimes_upd);
                otherwise
                    [p_KFStep_mid.x, p_KFStep_mid.P, tmp_K] = kalmfiltupd(p_KFStep_mid.x, p_KFStep_mid.P, in.zObs, in.H, in.GammaMRGammaMT);
            end
            p_KFStep_mid.updCnt = p_KFStep_mid.updCnt + 1;
            if  p_KFStep_cfg.enableNumCondCtrl
                p_KFStep_mid.P = covmatnumcondctrl(p_KFStep_mid.P, p_KFStep_par.PDiagMin, p_KFStep_par.PDiagMax);
            end
            % 输出
            p_KFStep_out.updCnt = p_KFStep_mid.updCnt;
            p_KFStep_out.x = p_KFStep_mid.x;
            p_KFStep_out.P = p_KFStep_mid.P;
            p_KFStep_auxOut.K = tmp_K;
        else
            warning('kalmfiltstep:chkFail', ['Measurement residule check and/or chi-square check failed, the observation after the ' int2str(p_KFStep_mid.updCnt) '-th update is ignored.']);
        end
    case int8(3)
        %% 状态向量重设
        p_KFStep_mid.x = p_KFStep_mid.x + in.u;
        if ~any(any(isnan(in.P0))) % TODO\LM: 判断是否有效使用NaN，下一行赋值中用到了P0的所有元素而非仅有对角线
            p_KFStep_mid.P = in.P0;
        end
        % 输出
        p_KFStep_out.x = p_KFStep_mid.x;
    otherwise
        %% 仅输出，不进行任何操作
end
out = p_KFStep_out;
auxOut = p_KFStep_auxOut;
end